#include <SoftwareSerial.h>
#include <math.h>
#include <Arduino.h>
#include <Wire.h>
#include <L3G4200D.h>
#include <LSM303.h>
#include <TimerThree.h>
#include <TimerOne.h>

#define rxPin 43
#define txPin 36

L3G4200D gyro;
LSM303 compass;

int rightSpeed = getSpeed(0, 0);
int leftSpeed = getSpeed(0, 0);

int leftMotor = 9;
int rightMotor = 10;
boolean leftOn = false;
boolean rightOn = false;

//Serial1 communicates with pin 19(RX) and pin 18 (TX) this controls the left motors
//Serial2 communicates with pins 17(RX) and 16(TX) this controls the right motors


//------------------------------------------>setup<---------------------------------------------------------------------
void setup()
{
  Serial.begin(9600);
  
  pinMode(leftMotor, OUTPUT);
  pinMode(rightMotor, OUTPUT);
  
  Timer1.initialize(100000); 
  Timer1.attachInterrupt(leftPulse); 
  
  Timer3.initialize(100000); 
  Timer3.attachInterrupt(rightPulse); 
  
  Wire.begin();
  gyro.enableDefault();
  
  compass.init();
  compass.enableDefault();
  
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass. (Used for heading, but not for other compass stuff?
  compass.m_min.x = -510; compass.m_min.y = -777; compass.m_min.z = -249;
  compass.m_max.x = +398; compass.m_max.y = +411; compass.m_max.z = 551;
}
//----------------------------------------------------------------------------------------------------------------------
//--------------------------------------->Read_IMU<---------------------------------------------------------------------
//Added- not tested
void read_imu()
{
  gyro.read();
  
  Serial.print("Y"); // gyroscopometer
  Serial.print("\\");
  Serial.print((int)gyro.g.x);
  Serial.print("|");
  Serial.print((int)gyro.g.y);
  Serial.print("|");
  Serial.print((int)gyro.g.z);
  Serial.println("|/");
  
  compass.read();
  
  Serial.print("A"); // accelerometer
  Serial.print("\\");
  Serial.print((int)compass.a.x);
  Serial.print("|");
  Serial.print((int)compass.a.y);
  Serial.print("|");
  Serial.print((int)compass.a.z);
  Serial.println("|/");
  
  Serial.print("P"); // magnetometer
  Serial.print("\\");
  Serial.print((int)compass.m.x);
  Serial.print("|");
  Serial.print((int)compass.m.y);
  Serial.print("|");
  Serial.print((int)compass.m.z);
  Serial.println("|/");
  
  int heading = compass.heading((LSM303::vector){0,-1,0}); // must be done -after- above
  Serial.print("H\\");
  Serial.print(heading);
  Serial.println("|/");
}
//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

void loop(){
  if(Serial.available()){
    byte val = Serial.read();
    if(val == 'I'){                                                      //ADDED 4/15
      read_imu();
    }else if(val == 'R'){
      int power = -1;
      int dir = 1;
      byte temp;
      delay(10);
      while(Serial.available() > 0){
        power = 0;
        temp = Serial.read();
        if(temp == '-'){
          dir = -1;
        }else if(temp >= '0' && temp <= '9'){
          power = power * 10 + temp - '0';
        }
        delay(10);
      }
      if(power >= 0 && power <= 100){
        Serial.print("Set right motor to ");
        Serial.println(dir * power);
        rightSpeed = getSpeed(dir, power);
      }
    }else if(val == 'L'){
      int power = -1;
      int dir = 1;
      byte temp;
      delay(10);
      while(Serial.available() > 0){
        power = 0;
        temp = Serial.read();
        if(temp == '-'){
          dir = -1;
        }else if(temp >= '0' && temp <= '9'){
          power = power * 10 + temp - '0';
        }
        delay(10);
      }
      if(power >= 0 && power <= 100){
        Serial.print("Set left motor to ");
        Serial.println(dir * power);
        leftSpeed = getSpeed(dir, power);
      }
    }
  }
}

int getSpeed(int dir, int percent){
  if(dir == 0 || percent == 0){
    return 1500;
  }else if(dir == 1){ // forward
    return map(percent, 0, 100, 1550, 2010);
  }else{ // reverse
    return map(percent, 0, 100, 1490, 1060);
  }
}

void leftPulse(){
  digitalWrite(leftMotor, digitalRead(leftMotor) ^ 1);
  if(leftOn){
    leftOn = false;
    Timer1.setPeriod(100000 - leftSpeed);
  }else{
    leftOn = true;
    Timer1.setPeriod(leftSpeed);
  }
}

void rightPulse(){
  digitalWrite(rightMotor, digitalRead(rightMotor) ^ 1);
  if(rightOn){
    rightOn = false;
    Timer3.setPeriod(100000 - rightSpeed);
  }else{
    rightOn = true;
    Timer3.setPeriod(rightSpeed);
  }
}

